﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;

namespace XNOgre
{
    public static class Extensions
    {
        public static void ToAxes(this Quaternion self, out Vector3[] axes)
        {
            axes = new Vector3[3];
            for (int i = 0; i < 3; i++)
            {
                axes[i] = new Vector3();
            }

            var rotation = Matrix.CreateFromQuaternion(self);

            axes[0].X = rotation.M11;
            axes[0].Y = rotation.M12;
            axes[0].Z = rotation.M13;

            axes[1].X = rotation.M21;
            axes[1].Y = rotation.M22;
            axes[1].Y = rotation.M23;

            axes[2].X = rotation.M31;
            axes[2].Y = rotation.M32;
            axes[2].Z = rotation.M33;

        }
        public static Quaternion RotationTo(this Vector3 vec, Vector3 dest)
        {
            return RotationTo(vec, dest, Vector3.Zero);
        }
        public static Quaternion RotationTo(this Vector3 vec, Vector3 dest, Vector3 fallBackAxis)
        {
            Quaternion q = new Quaternion();
            Vector3 v0 = vec;
            Vector3 v1 = dest;
            v0.Normalize();
            v1.Normalize();

            float d = Vector3.Dot(v0, v1);
            //If dot == 1, vectors are the same
            if (d >= 1.0f)
                return Quaternion.Identity;
            if (d < (0.000001f - 1.0f))
            {
                if (fallBackAxis != Vector3.Zero)
                {
                    q = Quaternion.CreateFromAxisAngle(fallBackAxis, (float)Math.PI);
                }
                else
                {
                    Vector3 axis = Vector3.Cross(Vector3.UnitX, vec);
                    if (vec.Length() == 0)
                        axis = Vector3.Cross(Vector3.UnitY, vec);
                    axis.Normalize();
                    q = Quaternion.CreateFromAxisAngle(axis, (float)Math.PI);
                }
            }
            else
            {
                float s = (float)Math.Sqrt((1 + d) * 2);
                float invs = 1f / s;

                Vector3 c = Vector3.Cross(v0, v1);

                q.X = c.X * invs;
                q.Y = c.Y * invs;
                q.Z = c.Z * invs;
                q.W = s * 0.5f;
                q.Normalize();
            }

            return q;
        }

        /// <summary>
        /// Creates a new BoundingBox with the bounds extended to the greater (max) and the lesser (min) of the passed in box.
        /// </summary>
        /// <param name="box"></param>
        /// <param name="other"></param>
        /// <returns></returns>
        public static BoundingBox Merge(this BoundingBox self, BoundingBox other)
        {
            BoundingBox box = new BoundingBox();
            box.Min = self.Min.Floor(other.Min);
            box.Max= self.Max.Ceil(other.Max);

            return box;
        }
        public static BoundingBox Merge(this BoundingBox self, Vector3 point)
        {
            BoundingBox box = new BoundingBox();

            box.Max = self.Max.Ceil(point);
            box.Min = self.Min.Floor(point);

            return box;
        }
        public static float GetBoundingRadius(this BoundingBox aabb)
        {
            Vector3 max = aabb.Max;
            Vector3 min = aabb.Min;

            Vector3 magnitude = max;
            magnitude = magnitude.Ceil(-max);
            magnitude = magnitude.Ceil(min);
            magnitude = magnitude.Ceil(-min);

            return magnitude.Length();
        }
        public static Vector3 Floor(this Vector3 self, Vector3 compare)
        {
            if (compare.X < self.X)
                self.X = compare.X;

            if (compare.Y < self.Y)
                self.Y = compare.Y;

            if (compare.Z < self.Z)
                self.Z = compare.Z;

            return self;
        }
        public static Vector3 Ceil(this Vector3 self, Vector3 compare)
        {
            if (compare.X > self.X)
                self.X = compare.X;

            if (compare.Y > self.Y)
                self.Y = compare.Y;

            if (compare.Z > self.Z)
                self.Z = compare.Z;

            return self;
        }
    }
}
